#include <aac.h>

flight_state_t flight_en=FLIGHT_DISABLE;

#define PWM_FREQ 10000
float standby_pwm=0;

float m1,m2,m3,m4;

float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;

float angle[3];//欧拉角
int16_t gyro[3],accel[3],gref[3];
float grad[3];
float greal[3],areal[3];
float roll_angle_req=0.0f,pitch_angle_req=0.0f,y_gyro_req=0.0f;
float roll_angle_add=0.0f,pitch_angle_add=0.0f;
int roll_pwm=0,pitch_pwm=0,yaw_pwm=0,pwm_vtl=0;

float pid_angle_roll[3]={4.0f,0.0f,0.0f};
float pid_angle_pitch[3]={4.0f,0.0f,0.0f};
float pid_angle_yaw[3]={0.0f,0.0f,0.0f};
float pid_gyro_roll[3]={4.0f,0.12f,20.0f};
float pid_gyro_pitch[3]={4.0f,0.12f,20.0f};
float pid_gyro_yaw[3]={5.0f,0.0f,0.0f};

static int IMUupdate(float gx, float gy, float gz, float ax, float ay, float az){
	static float exInt=0.0,eyInt=0.0,ezInt=0.0;
	static float Kp=6.8f,Ki=0.00f,halfT=0.0025f; //设置采样周期为5ms
	float tmp_q0=0.0,tmp_q1=0.0,tmp_q2=0.0,tmp_q3=0.0; //计算四元数缓冲
	float size=0.0; //归一化模
	float vx=0.0, vy=0.0, vz=0.0; //四元数估算重力分量大小
	float ex=0.0, ey=0.0, ez=0.0; 

	float  q0q0 = q0*q0;
	float  q0q1 = q0*q1;
	float  q0q2 = q0*q2;
	float  q0q3 = q0*q3;
	float  q1q1 = q1*q1;
	float  q1q2 = q1*q2;
	float  q1q3 = q1*q3;
	float  q2q2 = q2*q2;
	float  q2q3 = q2*q3;
	float  q3q3 = q3*q3;

	size = sqrt(ax*ax + ay*ay + az*az);      
	ax = ax /size;
	ay = ay / size;
	az = az / size; //加速度计算重力分量大小
			
	vx = 2*(q1q3 - q0q2);												
	vy = 2*(q0q1 + q2q3);
	vz = q0q0 - q1q1 - q2q2 + q3q3 ;

	ex = (ay*vz - az*vy) ;                           					
	ey = (az*vx - ax*vz) ;
	ez = (ax*vy - ay*vx) ;

	exInt = exInt + ex * Ki;								 
	eyInt = eyInt + ey * Ki;
	ezInt = ezInt + ez * Ki;
	/*
	if(fabs(ex)<=0.01)
		exInt=0;

	if(fabs(ey)<=0.01)
		eyInt=0;

	if(fabs(ez)<=0.01)
		ezInt=0;*/
	gx = gx + Kp*ex + exInt;					   							
	gy = gy + Kp*ey + eyInt;
	gz = gz + Kp*ez + ezInt;				   							
						
	tmp_q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
	tmp_q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
	tmp_q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
	tmp_q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT; //一定要使用缓冲

	q0=tmp_q0;
	q1=tmp_q1;
	q2=tmp_q2;
	q3=tmp_q3;

	size	= sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);

	q0 = q0 / size;
	q1 = q1 / size;
	q2 = q2 / size;
	q3 = q3 / size;
		angle[0] = asinf(-2 * q1 * q3 + 2 * q0* q2)* 57.296f;	// pitch
		angle[1]  = atan2f(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.296f;	// roll
		printf("%f,%f\r\n",angle[0],angle[1]);	
	//	angle[2]   = atan2f(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.296f;	//yaw 不使用角度控制yaw
		
}
const double num[7] = {//b
	0.002585064184237,  0.01551038510542,  0.03877596276356,  0.05170128368475,
    0.03877596276356,  0.01551038510542, 0.002585064184237
};
const double den[7] = {//a
	1,   -2.379721044555,    2.910406567865,   -2.055131436773,
	0.8779238976341,  -0.2098654503597,  0.02183157397997
};
//7阶iir直接I型滤波
float iir_lpf_accx(float input)
{ 
	static double y_out[7]={0}; //y0为最新
  static double x_in[7]={0};  //x0 为最新
	char i=0;
	for(i=6;i>=1;i--){
		x_in[i]=x_in[i-1];
	}
	for(i=6;i>=1;i--){
		y_out[i]=y_out[i-1];
	}
	
	x_in[0]=input; //输入当前值
	y_out[0]=0;
	for(i=1;i<=6;i++){
    y_out[0]+=num[i]*x_in[i]-den[i]*y_out[i];
	}
	y_out[0]+=num[0]*x_in[0];
	
	return (float)y_out[0];//返回滤波值
	
}
float iir_lpf_accz(float input)
{ 
	static double y_out[7]={0}; //y0为最新
  static double x_in[7]={0};  //x0 为最新
	char i=0;
	for(i=6;i>=1;i--){
		x_in[i]=x_in[i-1];
	}
	for(i=6;i>=1;i--){
		y_out[i]=y_out[i-1];
	}
	
	x_in[0]=input; //输入当前值
	y_out[0]=0;
	for(i=1;i<=6;i++){
    y_out[0]+=num[i]*x_in[i]-den[i]*y_out[i];
	}
	y_out[0]+=num[0]*x_in[0];
	
	return (float)y_out[0];//返回滤波值
	
}
float iir_lpf_accy(float input)
{ 
	static double y_out[7]={0}; //y0为最新
  static double x_in[7]={0};  //x0 为最新
	char i=0;
	for(i=6;i>=1;i--){
		x_in[i]=x_in[i-1];
	}
	for(i=6;i>=1;i--){
		y_out[i]=y_out[i-1];
	}
	
	x_in[0]=input; //输入当前值
	y_out[0]=0;
	for(i=1;i<=6;i++){
    y_out[0]+=num[i]*x_in[i]-den[i]*y_out[i];
	}
	y_out[0]+=num[0]*x_in[0];
	
	return (float)y_out[0];//返回滤波值
	
}
static int angle_PID_roll(float target,float real){//动态I增量控制
	static float err=0,last_err=0; //定义当前和历史偏差
	static float Kp=3.5f,Kd=0.0f,Ki=0.0f;
	static float out=0,inter=0;
	static uint8_t i_en=0;
	last_err=err;
	err=real-target;
	inter+=err;
	//积分分离
	if(err>=35&&err<=-35)
		i_en=0;
	else
		i_en=1;
	if(inter>=200)
		inter=200;
	else if(inter<=-200)
		inter=-200;
	// out=Kp*err+Kd*(err-last_err)+inter*Ki;
	out=pid_angle_roll[0]*err+inter*pid_angle_roll[1]+pid_angle_roll[2]*(err-last_err);
	if(out>=1000)
		out=1000;
	else if(out<=-1000)
		out=-1000;	
	return (int)out;
}
static int angle_PID_pitch(float target,float real){ //动态I增量控制 
	static float err=0,last_err=0; //定义当前和历史偏差
	static float Kp=3.5f,Kd=0.0f,Ki=0.0f;
	static float out=0,inter=0;
	static uint8_t i_en=0;
	last_err=err;
	err=real-target;
	inter+=err;
	//积分分离
	if(err>=35&&err<=-35)
		i_en=0;
	else
		i_en=1;
	if(inter>=200)
		inter=200;
	else if(inter<=-200)
		inter=-200;
	// out=Kp*err+Kd*(err-last_err)+inter*Ki;
	out=pid_angle_pitch[0]*err+inter*pid_angle_pitch[1]+pid_angle_pitch[2]*(err-last_err);
	if(out>=1000)
		out=1000;
	else if(out<=-1000)
		out=-1000;	
	return (int)out;
}
static int gyr_PID_roll(float target,float real){  //位置PD控制
	static float err=0.0f,last_err=0.0f; //定义当前和历史偏差
	// static float Kp=0.38f,Kd=10.5f,Ki=0.04f;
	static float out=0.0f,inter=0.0f;
	static uint8_t i_en=0;
	last_err=err;
	err=real-target;
	inter+=err;
	//积分分离
	if(err>=150&&err<=-150)
		i_en=0;
	else
		i_en=1;
	//积分限幅
	if(inter>=1200)
		inter=1200;
	else if(inter<=-1200)
		inter=-1200;
	//位置式pid
	// out=Kp*err+i_en*inter*Ki+Kd*(err-last_err);
	out=pid_gyro_roll[0] * err +inter * pid_gyro_roll[1] + pid_gyro_roll[2] * (err - last_err);
	if(out>1000)
		out=1000;
	else if(out<-1000)
		out=-1000;
	return (int)out;
}
static int gyr_PID_pitch(float target,float real){  //位置PD控制
	static float err=0.0f,last_err=0.0f; //定义当前和历史偏差
	// static float Kp=0.38f,Kd=10.5f,Ki=0.04f;
	static float out=0.0f,inter=0.0f;
	static uint8_t i_en=0;
	last_err=err;
	err=real-target;
	inter+=err;
	//积分分离
	if(err>=150&&err<=-150)
		i_en=0;
	else
		i_en=1;
	//积分限幅
	if(inter>=1200)
		inter=1200;
	else if(inter<=-1200)
		inter=-1200;
	//位置式pid
	// out=Kp*err+i_en*inter*Ki+Kd*(err-last_err);
	out=pid_gyro_pitch[0] * err +inter * pid_gyro_pitch[1] + pid_gyro_pitch[2] * (err - last_err);
	if(out>1000)
		out=1000;
	else if(out<-1000)
		out=-1000;
	return (int)out;
}
static int gyr_PID_yaw(float target,float real){
    static float err=0,last_err=0;
	static float Kp=0.75f,Kd=79.0f;
	static float out=0;
	last_err=err;
	err=real-target;
	// out=Kp*err+Kd*(err-last_err);
	out=pid_gyro_yaw[0]*err+pid_gyro_yaw[2]*(err-last_err);
	if(out>=600)
		out=600;
	else if(out<=-600)
		out=-600;
	return (int)out;
}

int cnt=0;
int aac_callback(void *ctx){
	int m1i,m2i,m3i,m4i;
		MPU_Get_Gyroscope(gyro);
		MPU_Get_Accelerometer(accel);
		greal[0]= (float)(gyro[0]-gref[0])/16.4f;
		greal[1]=-(float)(gyro[1]-gref[1])/16.4f;
		greal[2]=-(float)(gyro[2]-gref[2])/16.4f;
		grad[0]= (float)(gyro[0]-gref[0])/940.1274f;
		grad[1]=-(float)(gyro[1]-gref[1])/940.1274f;
		grad[2]=-(float)(gyro[2]-gref[2])/940.1274f;
		areal[0]=iir_lpf_accx((float)accel[0])/8192.0f;
		areal[1]=-iir_lpf_accy((float)accel[1])/8192.0f;
		areal[2]=-iir_lpf_accz((float)accel[2])/8192.0f;
		IMUupdate(grad[0],grad[1],grad[2],areal[0],areal[1],areal[2]);
		roll_angle_add=angle_PID_roll(roll_angle_req/30,angle[0]);
		pitch_angle_add=angle_PID_pitch(pitch_angle_req/30,angle[1]);
		roll_pwm=gyr_PID_roll(-roll_angle_add,greal[1]);	
		pitch_pwm=gyr_PID_pitch(-pitch_angle_add,greal[0]);
		yaw_pwm=gyr_PID_yaw(y_gyro_req,-greal[2]);

		//四个电机赋值
		m1i=pwm_vtl-roll_pwm-pitch_pwm+yaw_pwm;
		m2i=pwm_vtl-roll_pwm+pitch_pwm-yaw_pwm;
		m3i=pwm_vtl+roll_pwm+pitch_pwm+yaw_pwm;
		m4i=pwm_vtl+roll_pwm-pitch_pwm-yaw_pwm;
		if(m1i>1000)m1=1000;
		else if(m1i<0)m1=0;
		else m1=m1i;
		if(m2i>1000)m2=1000;
		else if(m2i<0)m2=0;
		else m2=m2i;
		if(m3i>1000)m3=1000;
		else if(m3i<0)m3=0;
		else m3=m3i;
		if(m4i>1000)m4=1000;
		else if(m4i<0)m4=0;
		else m4=m4i;
	
    if(flight_en==FLIGHT_DISABLE){
        pwm_set_frequency(PWM_DEVICE_1, PWM_CHANNEL_0, PWM_FREQ,0);
        pwm_set_frequency(PWM_DEVICE_1, PWM_CHANNEL_1, PWM_FREQ,0);
        pwm_set_frequency(PWM_DEVICE_1, PWM_CHANNEL_2, PWM_FREQ,0);
        pwm_set_frequency(PWM_DEVICE_1, PWM_CHANNEL_3, PWM_FREQ,0);
    }
    else if(flight_en==FLIGHT_STANDBY){
        pwm_set_frequency(PWM_DEVICE_1, PWM_CHANNEL_0, PWM_FREQ,standby_pwm);
        pwm_set_frequency(PWM_DEVICE_1, PWM_CHANNEL_1, PWM_FREQ,standby_pwm);
        pwm_set_frequency(PWM_DEVICE_1, PWM_CHANNEL_2, PWM_FREQ,standby_pwm);
        pwm_set_frequency(PWM_DEVICE_1, PWM_CHANNEL_3, PWM_FREQ,standby_pwm);
    }
	else if(flight_en==FLIGHT_ENABLE){
        pwm_set_frequency(PWM_DEVICE_1, PWM_CHANNEL_0, PWM_FREQ,m1/1000);
        pwm_set_frequency(PWM_DEVICE_1, PWM_CHANNEL_1, PWM_FREQ,m2/1000);
        pwm_set_frequency(PWM_DEVICE_1, PWM_CHANNEL_2, PWM_FREQ,m3/1000);
        pwm_set_frequency(PWM_DEVICE_1, PWM_CHANNEL_3, PWM_FREQ,m4/1000);
	}
	return 0;
}
void AAC_Init(){
	int i=0;
	// msleep(500);
	// mpu_dmp_read_gyroscope(gyroref);
	while(mpu6050_init()!=0);
	for(i=0;i<100;i++){
		msleep(1);
		MPU_Get_Gyroscope(gref);
	}
    timer_init(TIMER_DEVICE_0);
    timer_set_interval(TIMER_DEVICE_0, TIMER_CHANNEL_0, 5e6);
    timer_irq_register(TIMER_DEVICE_0, TIMER_CHANNEL_0, 0, 1, aac_callback, NULL);
    timer_set_enable(TIMER_DEVICE_0, TIMER_CHANNEL_0, 1);
    fpioa_set_function(32, FUNC_TIMER1_TOGGLE1);
    fpioa_set_function(33, FUNC_TIMER1_TOGGLE2);
    fpioa_set_function(34, FUNC_TIMER1_TOGGLE3);
    fpioa_set_function(35, FUNC_TIMER1_TOGGLE4);
    pwm_init(PWM_DEVICE_1);
    pwm_set_frequency(PWM_DEVICE_1, PWM_CHANNEL_0, PWM_FREQ,0);
    pwm_set_frequency(PWM_DEVICE_1, PWM_CHANNEL_1, PWM_FREQ,0);
    pwm_set_frequency(PWM_DEVICE_1, PWM_CHANNEL_2, PWM_FREQ,0);
    pwm_set_frequency(PWM_DEVICE_1, PWM_CHANNEL_3, PWM_FREQ,0);
    pwm_set_enable(PWM_DEVICE_1, PWM_CHANNEL_0, 1);
    pwm_set_enable(PWM_DEVICE_1, PWM_CHANNEL_1, 1);
    pwm_set_enable(PWM_DEVICE_1, PWM_CHANNEL_2, 1);
    pwm_set_enable(PWM_DEVICE_1, PWM_CHANNEL_3, 1);
};